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Abstract. A Hamiltonian algorithm, both theoretical and numerical, to obtain 
the reduced equations implementing Pontryagine's Maximum Principle for singu- 
lar linear-quadratic optimal control problems is presented. This algorithm is in- 
spired on the well-known Rabier-Rheinhboldt constraints algorithm used to solve 
differential-algebraic equations. Its geometrical content is exploited fully by im- 
plementing a Hamiltonian extension of it which is closer to Gotay-Nester presym- 
plectic constraint algorithm used to solve singular Hamiltonian systems. Thus, 
given an optimal control problem whose optimal feedback is given in implicit form, 
a consistent set of equations is obtained describing the first order differential con- 
ditions of Pontryaguine's Maximum Principle. Such equations are shown to be 
Hamiltonian and the set of first class constraints corresponding to controls that 
are not determined, are obtained explicitly. 

The strength of the algorithm is shown by exhibiting a numerical implementa- 
tion with partial feedback on the controls that provides a partial optimal feedback 
law for the problem. The numerical algorithm is inspired on a previous analysis 
by the authors of the consistency conditions for singular linear-quadratic optimal 
control problems. The numerical algorithm is explicitly Hamiltonian. It is shown 
that only two possibilities can arise for the reduced Hamiltonian PMP: that the 
reduced equations are completely determined or that they depend on a family of 
free parameters that will be called the "gauge" controls of the system determined 
by the first class constraints. The existence of these extra degrees of freedom opens 
new possibilities for the search of solutions. Numerical evidence of the stability of 
the algorithm is presented by discussing various relevant numerical experiments. 
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1. Introduction 

In a recent paper [6] it was shown that the reduced first order differential con- 
ditions of Pontryagine's Maximum Principle (PMP) for singular optimal control LQ 
problems can be obtained by using a stable linear numerical algorithm inspired in the 
theory of constraints whose origin lies in the theory of singular Lagrangians on ana- 
lytical mechanics. However that numerical algorithm was not compatible in general 
with the natural Hamiltonian structure of those systems. 

In this paper we will present an algorithm to obtain the differential conditions 
implementing PMP for singular optimal control problems (both theoretically and 
numerically) that is inspired too in the theory of constraints but is explicitly Hamil- 
tonian. In order to achieve it, a new Hamiltonian structure is introduced that provide 
an explicit Hamiltonian structure for the PMP. Such Hamiltonian structure is the lin- 
ear counterpart of the construction known in symplectic geometry as the coisotropic 
embbeding of presymplectic manifolds [ID]. This extended Hamiltonian structure 
allows in addition to analyze the reduced PMP by using the standard classification 
of the constraints of the problem in first and second class. 

We will recall that an optimal control problem is singular if there is no optimal 
feedback law or, in other words, if the first order differential conditions imposed by 
Pontryagine's Maximum Principle define an implicit system of differential equations. 

It is natural to approach such situation by considering a reduction of the system 
imposing the consistency of the corresponding set of differential-algebraic equations. 
Such procedure can be carried out in various forms (see for instance [12] and references 
therein). A geometrical way of describing the consistency of implicit systems was 
discussed by Rabier and Rheinboldt [IT] (see also [14"]). 

These algorithms are broad generalizations of Gotay's and Nester presymplec- 
tic constraint algorithm (PCA) [8] that allows to find the solutions of presymplec- 
tic Hamiltonian systems and that provides a geometrical framework to the Dirac- 
Bergmann theory of constraints for singular Lagrangian systems, i.e., Lagrangian 
systems such that the Legendre transform is not a diffeomorphism |7J. Lopez and 
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Martinez |13j used it to discuss problems in subriemannian geometry. Volckaert 
already started the use of the PC A algorithm to study singular optimal control prob- 
lems [20] and Petit [15] used the Rabier-Rheinboldt algorithm to solve a class optimal 
control problems with control equation in implicit form. Delgado and Ibort proposed 
an analysis of singularities and the use of constraint algorithms in the realm of opti- 
mal control theory [U [5] and more recently Barbero et al [2j have discussed the PCA 
algorithm in this setting again. 

The numerical implementation of the constraint algorithm discussed in [6j produce 
the largest subspace where the implicit differential equations defined by the PMP are 
consistent and in the setting of LQ optimal control provides an alternative analysis to 
the index and the structure of the system [I], The constraint algorithm becomes 
a linear algebra problem and can be suitably dealt with by an appropriate use of the 
SVD algorithm as in [6], however such reduction algorithm didn't take advantage 
of all the information provided by the SVD decomposition. In fact at any step of 
the algorithm a number of control variables can be solved explicitly and reinserted 
in the algorithm. We will call such mechanism a constraints algorithm with partial 
feedback on the controls and will be developed in Sections 1 and 2 of this paper. 

Moreover, as it was indicated above, the general numerical solution of DAE's 
(see for instance [3] and references therein) doesn't preserve the Hamiltonian (or 
symplectic) structure of the problem. In Section 3 the presymplectic geometry of 
the problem is reviewed and a new Hamiltonian regularization of the problem is 
presented that will be used to develop a Hamiltonian constraints algorithm that 
extends the constraints algorithm with partial feedback, and thus it is shown to be 
Hamiltonian. The Hamiltonian extension of the constraints algorithm allows for a 
complete description of the structure of the reduced PMP by introducing the notion 
of first and second class constraints. It will be shown that the reduced equations are 
not completely determined unless there are no first class constraints and that the 
undetermined degrees of freedom are just determined explicitly by them. 

Section 4 analyzes the numerical implementation of this Hamiltonian regulariza- 
tion for singular LQ optimal control problems. In Section 5 the pseudocodes of the 
various algorithms are discussed in detail and, finally, in Section 6 various relevant 
numerical experiments are presented that explore some of the different situations that 
arise in the theory and that provide evidence for the stability of the algorithm. 

2. A RECURSIVE CONSTRAINTS ALGORITHM WITH PARTIAL FEEDBACK ON THE 



2.1. The setting. As it was stated in the introduction, we will study LQ optimal 
control systems (even if many of the theoretical results obtained here remain valid in 
more general settings), more specifically we will discuss the problem of finding C 1 - 
piecewise smooth curves j(t) = (x(t),u(t)) satisfying the linear control equations^} 



CONTROLS FOR SINGULAR LQ SYSTEMS 



A)x^+B l a u\ i = l,..., 



n, a = l,...,r, 



(1) 



minimizing the objective functional: 




to 



Einstein's summation convention on repeated indices will be used in what follows unless stated 
explicitly. 
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where the quadratic Lagrangian L has the form: 

L(x, u) = ^Qij x { x j + N ia x i u a + -R ab u a u\ (3) 

the matrices Q and R being symmetric, and subjected to fixed endpoint conditions: 
x(to) = xo, x(T) = xt- The coordinates x l , i = l,...,n describe points in state 
space x E M. n , and u a , a = 1, . . . , m, are the control parameters defined on the linear 
control space M m . The matrices A, B, Q, N and R will be considered to be constant 
for simplicity even if the algorithm we are going to discuss can be applied without 
difficulty to the time-dependent situation. 

Normal extremals are provided by Pontryagine's Maximum Principle [16]: the 
curve (x(t), u(t)) is a normal extremal if there exists a lifting (x(t),p(t)) of x{t) to 
the costate space IR n x W 1 satisfying Hamilton's equations: 

x l = — •• = -— (4) 
dpi ' ^ l dx l ' 

where H is Pontryagine's Hamiltonian function: 

H(x,p,u) = Pi (A)x j + £> a ) - L(x,u), (5) 
and the set of conditions: 

<j>£\x,p,u) := — = Pi B i a -N ia x i -R ab u b = 0. (6) 

2.2. Consistency conditions and the recursive constraints algorithm. We 

will call conditions in Eq. ^ primary constraints. Trajectories that are solution to 
the optimal control problem must lie in the linear submanifold: 

Mi = {(x,p,u) G M 1 </>P(x,p,u) = 0}, (7) 
where Mo = { (x,p,u) £ ]j2n+m y denotes the total space of the system. 

If 7(i) = (x(t),p(t),u(t)) is a solution of the optimal problem, then its derivative 
will satisfy Eqs. Q and 

u a = C a (x,p,u), (8) 

with C a (x(t),p(t),u(t)) a function depending on the curve 7(i), however because of 
the constraint ^ we get, 

d£ } dH d£ ] oh d<t£ ] nb _ 



dx % dpi dpi dx l du b 
Then, if the matrix 

Rab _ g!g 

a du a du b 

is invertible in M\ , then the functions C a are completely determined and there exists 
an optimal feedback condition solving Eq. ([6]), given explicitly by: 

u h = {R- 1 ) ab {p l B\-N ia x i ). (9) 

and then, we obtain for Eq. Q: 

u b = (R- 1 ) ab {p i Bl-N ia x l ) = 

= (R^) ab ((- Pj 4 + Q ijX i + N id u d )Bl - N ia {A) xP + B\ u d ) 
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Notice that in this case (jxp vanishes automatically on M\ . Linear-quadratic systems 
such that the matrix R is invertible will be called in what follows regular and singular 
otherwise. 

For a singular optimal LQ system it may occur that solutions of Q starting at 
points (xq,po, uo) 6 M\ will not be contained in M\ for t > for any u. Because Eqs. 
Q-Q must be satisfied along optimal paths, we must consider initial conditions only 
those points {xq,Pq,Uq) for which there is at least a solution of Q contained in M\ 
starting from them, i.e., such that there exists C for which, 

# := j>P = 0, (10) 

where the derivative is taken in the direction of Eq. Q and u = C. The subset 

obtained, that will be denoted by M2, is again a linear submanifold of M\ (this is 

(2) 

also true in the time-dependent case). The functions <fi a defining M2 in M\ will be 
called secondary constraints, also known as "hidden constraints" in the context of 
DAEs. 

Clearly, the argument goes on and we will obtain in this form a family of linear 
submanifolds defined recursively as follows: 

M fc+ i = {(x,p,u) e M k I 3 C such that <p[ k+1) := <p{ k) (x,p,u) = 0}, k > 1. (11) 

where the derivative <^>a (x,p,u) it taken in the direction of eq. Q and ii a = C a on 
Mfc, i.e., satisfying all previous constraints I < k. Eventually, the recursion will 
stop and M K = M K+ \ = M K+ 2 = • • • , for certain finite k. We will call the number k 
of steps it takes the algorithm to stabilize, the (recursive) index of the problem. The 
final space M R := M K will be called the final constraint submanifold of the problem, 
that is the set of consistent initial condition for the DAE Q-Q. 

As it was stressed in the introduction, this algorithm constitutes an adapted ver- 
sion to the present setting of the reduction algorithm for DAEs [T71 [18] (see also [12] 
and [3]). However, the DAE system above has an additional geometrical structure. 
We will devote the following sections to incorporate this additional structure in the 
algorithm, but before doing it we will discuss a version of this algorithm that incor- 
porates a partial feedback of the controls. The idea behind it is that depending on 
the rank of the matrix of coefficients of the controls in the constraints, part of them 
could be solved providing a partial optimal feedback law. This idea will be discussed 
in the following section. 

2.3. Constraints algorithm with partial feedback on the controls. We will 
use matrix notation in order to write in a compact form the recursive law for the 
algorithm. 

2.3.1. The initial data. We will denote by := M = R 2n x R m the total space of 
the system where x, p G M. n and :=a£ M m ° (mo := m) denote column vectors. 

Thus the problem we have is to obtain solutions of the differential equation: 

x = Ax + Bu, 

with A, Q € R nxn , B, N £ R nxm and R £ R mxm , such that Pontryagine's Hamil- 
tonian is maximized: 

H(x,p,u)=p Ax + p Bu — —x Qx — x Nu—-u Ru. 
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Thus, the differential conditions for the PMP, eq. Q become: 

Qx - A T p + Nu, u = C. 



dH dH 

x= 1 -^ = Ax + Bu, p = -— r 
op 1 ox 1 



We may written the previous differential equation as a vector field depending on the 
parameters u as: 

Ax + Bu 
Qx - A T p + Nu 



r(°>(«) 



X 




. p _ 





= G (0) 


X 




. p . 



+ 



with 



G (o) 



z (o) 



A 

The primary constraint, eq. Q, written as column vector is given by: 



B 

N 



(i) - 



-N T x + B T p -Ru = S {1) 



x 
P 



with 



S« = [-iV T |i? T ] = -Z( ) T J, i?« =J R, 
and J denotes the 2n x 2n canonical symplectic matrix: 

J 



(12) 






In 


-In 






Because of the linear nature of the equations above, all further constraints derived 
in the algorithm will be linear. 

2.3.2. The partial feedback. If is singular we implement the recursive constraints 
algorithm as follows. First we compute the SVD of R.W 



R W = v w 





' 









y(l)T 



with X^ 1 ) the diagonal matrix of rank r\ whose diagonal elements are the singular 
values si, . . . s ri of R^\ We will redefine the controls u^ as follows: 

: = : ri ), u« := (V« T «(°))(n + 1 : m ), 



in other words: 



;(o) 



V WT u (0) 



Then multiplying the primary constraint <f>^ by U^ T on the left: 

fi(0) 



o = uW T <i>w = uW T sW 



X 




r sw 


" 


p _ 











(13) 



(14) 



X 




■ S (i) fi (0) - 


. p _ 


+ 






The previous equation splits in two parts: 
(1) The partial feedback: 

fi(0) = ( E (l))-l^(l)T (1:rij . )S (l) 



X 


= : Feed* 1 * 


X 


. p _ 




. P . 



(15) 
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(2) The reduced primary constraint: 

4>W =uU T ( ri + l:m,:)SM 

We may write: 



P 



(16) 



S f = U^ T (1 : n, :)SW, S& = U^ T (n + 1 : m, 



this is, 



S 



(i) 



With this notation, the matrix Feed*- 1 -* determining partial feedback of the controls 
u(°) reads: 

Feed (D = (EW)- 1 ^ 1 ), 
and the reduced primary constraint 4>^: 



p 



0. 



(17) 



If we denote by mi the length of the vector vP\ i.e., mi = tuq — n, then we may 
consider that our optimal control problem is defined on the space = R 2n x R mi 
with elements (x,p,u^). There the reduced primary constraint (fP^ defines a linear 
submanifold Mi = {(x,p,u^) € M (1) | = 0}. This submanifold M x is clearly 
isomorphic to M\ by using the map: 

a\\ Mi — >Mi, ai(x,p,n (1) ) = (x,p,u^) 

with: 



(18) 



u 



(0) = 



„(0) 



, u(°)=Feed( 1 ) 



P 



2.3.3. T/ie first iteration. The identification between Mi and Mi allows to write the 
differential equation T^°\u) as a vector field on Mi, in other words, if we introduce 
the feedback for the controls u° on the equations for x and p become equations just 
on the controls We will denote the vector field thus defined on Mi as T^\u^) 
(or simply rW if it is not necessary to make explicit the dependence on the control 
parameters u^), then: 

+ z (0V(i)U (0) 



p(l) = Q(0) 



X 

p 



u 



Writting 



this is: 



Z(0) := ( Z (°)y( 1 ))(l : n, :), ZW := {Z^V^)(n + 1 : m , :), 



z(°) 



we will get finally: 

rW = G (0) 



P 



x 
P 
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with 



G (D = G (o) + ^(o) Feed (i)_ 



Recall that the consistency of the differential equations given by r<°)(u) on Mi or 
equivalently, by I^ 1 ) on Mi, determines the secondary constraints, i.e., the submani- 
fold M2, etc. Thus imposing consistency to the reduced primary constraint <fft~\ eqs. 



(16) or (17), we get the secondary constraint: 



X 




X 




X 




. p _ 


. P _ 




. P _ 





= 0( 2 ) = f ] = s« 

(19) 

with the new coefficients: 

S( 2 ) = RW = -SPzW. (20) 

At this point, we will iterate the procedure, this is, if R^ is regular, we will solve 
the controls and the algorithm stops. If R^ is singular we will compute its SVD 
again: 

p(2) = r/(2) f" S(2) I 1 y(2)T 

[ I J 

with £( 2 ) or rank r2, and we decompose the controls u^- 1 ' as: 



w 

where has length T2 and has lenght m2 = mi — r2 = — r\ — r-i- Then we 
will obtain the decompositions: 

?(2) 

l_ t Z (l)y(2) = ^(1) J Z (2)]_ 



jy(2)Tg-(2) 



5 



5, 



(27 



Then we will decompose the modified secondary constraint U( 2 ' T <fi( 2 ' into the feed- 
back part and the new reduced constraint (ft^ , obtaining respectively: 



fiW = Feed (2) 



x 



P 



Feed( 2 ) = (S( 2 ))- x 4 2) , 



and 



Then, the vector field r( 2 )(u( 2 )) is given by: 



P 



r (2) = G (2) 



P 



+ Z {2) U {2)^ G (2) = G .(l) + ^(l) Fe e d (2) ; 



and the stability condition for the reduced secondary constrains 4>^ gives the tertiary 
constraints: 



xJ2) 



p 



where 



s® = sj?>G<®, r^ = -sWzV\ 

and the next iteration follows easily. 
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2.3.4. The general iteration. At the kth iteration of the algorithm, the matrices: 

G^\z ik \u (k \v ik \S {k) 
will be given, as well as the decompositions: 



lj(k)T s (k) 



S 



(k) 

JL 



and 



Z {k)y{k) = ^(k) | z (k+i)^ 
The vector field will be given by: 



V 



r (fc) = G (k) 
and the reduced &:-ary constraints will be: 



+ Z^u { - k \ 



(21) 
(22) 



x 



V 



i2n 



xR mk 



The reduced constraints 0^ define a linear submanifold j k '■ M k — 

M k = {(x,p,u( k ~ 1 '>) e AfW = R 2n x R mk | = o}, (23) 
which is isomorphic to the linear submanifold M k by means of the map: 

a k :M k ^M k , a k {x,p, u (fc) ) = (x,p, u^" 1 )) (24) 

with: 



u 



(fc-i) 



u 



(fc-1) 



u 



u 



("-V = Feed^ 



P 



The consistency condition for the /c-ary reduced constraints will give the k + 1-ary 
constraints: 



~{k) 



with 



After computing the SVD factorization of R^ k+1 ^: 



R (k+i) = jjik+i) 



s (fc+l) 












y(fc+l)T 



(25) 

(26) 
(27) 



with £( fe+1 ) or rank r^+i, we decompose the controls as: 



u 



(k) 



u 



Ik+iy 



:= y(fc+i)Vfc) 



where u( fc ) has length r k+ i and u( fc+1 ) has lenght m k+ i = m k — r k+ i = mo — (fi + 
• • • + r k+ \). There again we will decompose the matrix S^ k+1 ^ according with: 

,(fc+i) 

jj(k+i)T s (k+i) = J / 
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Thus the partial feedback for the controls u k will be: 



= Feed( fc+1 ) 



P 



Feed( fc+1 ) = (E^ 1 ))-^^, 



the reduced (k + l)-ary constraints will be given by: 

_ s (k+i) 

and the new vector field: 

p(fc+l) _ Q(k+1) 



P 



+ z ( fc+ l) u ( fc+1 ) 



where 



(28) 



(29) 



G (k+i) = G (k) + z( k )FeeS k+1 \ (30) 

Now we are ready to proceed again as we have obtained the matrices (eqs. (30 ), ( plj ), 
@, ©): 

j7(*+i) j v(fc+i) ) S 1 ^ 1 ), 
that together with G^ 1 \Z^\U^\V^\E 1 ,S^ obtained in Section 



2.3.3 



will com- 



plete the recursive constraints algorithm with partial feedback on the controls. 



2.3.5. The total feedback. At the end, if n is the index of the problem, the total 
feedback is the collection of all partial feedbacks . . . , (u = u^°\ m = mo): 



#(1 : n) 
u (1) (l:r 2 ) 



(2) 



U 



u {K \l : r K ) 



u 



(k+1) 



Feed (1) 



x 
P 



V {1)T (ri + 1 : m,:)u 
V^ T (l:r 2 ,:)uW = 

V {2)T {1 : r 2 , :)F (1)T (ri + 1 : m, :)i 



Feed (1) 



x 



V {2)T (r 2 + 1 : mi,:)u (1) = 
V {2)T {r 2 + 1 : mi, :)V (1)T ( ri + 1 : m, :)u 

V^ T (l:r K+1 ,:)uW = 
V^ T (l:r K+1 ,:)V^ T (r K + l:m K ,:)--- 
V {2)T {r 2 + 1 : m 2 , :)y (1)T (ri + 1 : mi, :)u = Feed (K) 

V < - K+ ^ T (r K+1 + l:m K ,:)---V^ T (r 2 + l:m 1 ,:)V {1)T (ri + l:m,:)u 



The controls that remain free at the end, i.e., the vector u^ K+1 \ which are in a 
number m — (n + ■ — h ) , can be written a£ m( k+1 ) = Nofeed • u, where the matrix 
Nofeed is: 

Nofeed := V {K+1)T {r K+1 + 1 : m K , :) • • • F (2)T (r 2 + 1 : m 1; :)y (1)T (ri + 1 : m, :). 
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and the part of the controls that have been solved by using the successive partial 
feedbacks can be written as 



u = V ■ u = Feedtot 



P 



with 



V 



and 



VW T (1 :n,:) 
V^ T (1 : r 2 ,:)y (1)T (ri + 1 : m, :) 

V (n+l)T^ . rK+h :) . . . F (l)T( n + i : m? .) 
Feed {1) 

Feedtot 

Feed (K+1) 



3. The presymplectic structure of the PMP and the constraints 

ALGORITHM 

3.1. Presymplectic constraints algorithm for singular optimal control prob- 
lems. As it was mentioned previously, the differential conditions of the PMP have 
the geometrical structure of a presymplectic system. We will discuss it briefly in the 
particular instance of LQ systems (see for instance [2] for more details) . 

If we denote by P the state space of our system (in the case of LQ systems P = W 1 ) 
with local coordinates x 1 as above and by C the space of controls (in the case of LQ 
systems C = M m ) with coordinates u a , we can form the space Mq = T*P x C 
where T* P denotes the cotangent bundle of P (for LQ systems T* P = M 2n ) with 
local coordinates (x l ,pi) where pi are called costate variables. In the space Mq there 
is a canonical 2-form fio = dx % A dpi which is obtained by pulling-back along the 
projection map tt: Mo — *-T*P, ir(x,p,u) = (x,p), the canonical symplectic form 
uj = dx l A dpi defined on T*P. Notice that the 2-form is obviously closed but no 
non-degenerate because it has a kernel K := kerf^o which is spanned by the vectors 
d/du a . The triple (Mq,Qq, Hq) defines a presymplectic Hamiltonian system with 
Hq(x,p, u) Pontryagine's Hamiltonian. The dynamics is provided by any vector field 
T such that 



ir^o = dHo 



(31) 

j>2n+m^ p reS y m plectic 



Notice that in the case of LQ systems the space Mo is 
2-form f^o is constant. Again in the particular instance of LQ systems, if we consider 
the vector field 

„ .id .8 . „ d 



as a (column) vector in 



l = x —— 

ox 1 

i 2n+m and 



+ Pi 



dp, 



du° 



dH n 



dH, 



dx l 



dx l + -rr^-dpi + -^rdu a 



dpi 



du a 
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as a vector on . 



o2n+m 



using the identification between vectors and covectors provided 
by the given choice of a basis, the dynamical equation, eq. (|3 1 h take the matrix form: 






-In 


" 




X 




' dH /dx T ' 


In 










p 




dHo/dp 1 













ii 




dH /du r 



(32) 



which is equivalent to eqs. Q-Q (in fact the previous equation is the local form in 
canonical coordinates of any presymplectic system). 



These equations, eqs. (31) or (32), have the form of a quasilinear system on Mq 

A(()C = F(C) (33) 



where £ denotes the set of coordinates (x l ,pi,u a ), A(Q is the (constant) matrix [Q] 
and -F(C) is the vector VHq. In general a vector ( satisfying eq. (33) will not exists 
for all £'s. A necessary and sufficient condition for the existence of £ at a given point 
C is that (Z, -F(C)) = 0, for all Z E ker ^4(C)- In the case of the presymplectic system 
eq. (31), this condition means that at a given point £ £ Mo the vector V will exists 
iff cLHq(Z) = for all Z G K. The points at which such vector V exists determines a 
subset Mi of Mq. A family of independent functions defining M\ are called primary 



constraints ^ a 
the vectors Z a 
by: 



(i) 



for the presymplectic system. Notice that because K is spanned by 
= d/du a , then a family of independent primary constraints are given 



$\x,p,u) = dH (Z a ) = dH /du a = 0. 



(34) 

For LQ systems the subspace Mi was discussed in Section 2.2 and the primary con- 
straints (p^ take the explicit expressions given in eq. ([6]). 

On Mi the presymplectic form £Iq induces by restriction a new presymplectic form 
Oi = £Iq \mx- If we denote by i\ : Mi — >Mq the canonical inclusion map we can also 
write Oi = i\£lQ. The restriction of Pontryagine's Hamiltonian to Mi will be written 
Hi = Hq I j\,fi- Hence we have obtained a new presymplectic system (Mi,Qi, H\) on 
Mi. The corresponding dynamical equation is now given by: 

ir^i = dHi, 

and we should restrict again to the subset M2 of Mi where Ti exists. We will repeat 
the argument for M2 and we proceed iteratively afterwards. 

At each step k of the algorithm, the presymplectic form $1q restricts to the sub- 
manifold Mfc by inducing a new presymplectic structure fi^ = Qq \M k on it as well as 
Pontryagine's Hamiltonian 



H o \M k - 



The dynamical equation (31) becomes: 
= dHk, 



(35) 



Thus the system (35) will be consistent, i.e., there will exist a vector V satisfying eq. 
(35), if the constrainsts defined by this system are fullfilled, this is dHk(Z) = for 
all Z G kerfijfc. Let us denote by K k = kerOfc. Hence the fc-ary constrainsts 0( fc ) will 
be: 

(f) {k+l) (x,p,u) = dH k (Z) = 0, \JZeK k . (36) 

Thus at each step the algorithm has the structure of a presymplectic system which 
is consistent on the subspace: 

M fc+ i = {{x,p, u) € M fe I dH k (Z) = 0, \/Z G K k }, k > 1. (37) 
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This constitutes the recursive presymplectic constraints algorithm (or presymplectic 
algorithm for short) given in [8]- [9]. We will denote by i k +i ■ M k+ \ — ^M k the natural 
inclusion. Notice that £lk+i = i* k+ \^k-, etc. 

The presymplectic algorithm above, eq. (37) is equivalent to the recursive con- 
straints algorithm eq. (11), however it is not immediately obvious that they are 



equivalent to the constraints algorithm with partial feedback discussed in Section 
2.3 The following theorem shows that they are all indeed the same. 



Theorem 1. The presymplectic and the recursive constraints algorithms are equiv- 
alent. Moreover, for singular LQ systems, the constraints algorithm with partial 
feedback on the controls is equivalent to the presymplectic algorithm. 

Proof. We will proof the equivalence of both algorithms by induction on k, the step 
of the algorithm. For k = 1 is obvious as the expression of the primary constraints 
(p^\ hence of M\ is the same for both algorithms. Let us consider the fcth step of the 
algorithm. Thus we have the presymplectic system (M k , Q k , H k ) and the consistency 

G M k , then there will 
define 



condition eq. (|36|). If eq. (|36|) is satisfied at the point (x,p,u 

thus because the constraints 



(35) 



Conversely, if eq. (11) is verified at some 



exists a vector T in M k satisfying eq 
M fc , then T((j)^) = at (x,p,u) G M k . 
point (x,p,u) G Mk, then there exist C a such that the vector in M k given by: 

dH k d dH k d „„ d 



r(x,p, u) 



+ c a 



du a 



dpi dx l dxi dpi 

satisfies T((j)^) = 0. However, this vector T clearly satisfies eq. (35), and then 
necessarily at the point (x,p,u) it must be satisfied that dH k {Z) = for all Z G K k . 

We will prove now the equivalence of the constraints algorithm with partial feed- 
back on the controls and the previous two algorithms by induction on k. At the 
first step of the algorithm the spaces M\ and M\ are linearly isomorphic by the map 
«i (eq. (|18[)). It was shown in Section 2.3.3 that the consistency conditions for the 
reduced constraints <f>^ with respect to the vector field were equivalent to the 
secondary constratints <f>^ defining M^ (eq. (|19[)) hence the equivalence at step 1. 

Now suppose that we are at the step k of the algorithm, thus we have on one side 
the subspace M/% defined by the /c-ary constraints (f>^ and the subspace M k C M^ 
defined by the reduced fc-ary constraints (p^ and the map a k '■ M k — ^M k given by eq. 



(24) is a linear isomorphism. Now the constraints algorithm consistency condition, 
eq. (11), for (j)^ implies that there exist C a such that (f)^ = when the derivative 



is taken in the direction of the vector 

d 



r 







dx T ^ dp 



+ C 



d 
du T 



this is: 



P 



R^C. 



However by using the SVD factorization of we see immediately that there exists 
a solution of the previous linear equation if and only if: 



= si k) 



P 
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but this consistency condition for the partial feedback constraints algorithm, eq. 



(25), defines both the partial feedback eq. (28) and the k + 1-ary reduced con- 



straints, eq. (29), this is Mk+i- Moreover the map a^+i : M^+i — >■ M^ + i defined by 
otk+i(x,p, u( k ^>) = (x,p,u k ) with = [u^; u^ fc+1 - > ] and vS k ^ given by the partial 



feedback eq. (28) is a linear isomorphism and the equivalence is proved. □ 



When the algorithm stabilizes, this is after the step k we have: M K = M K+ \ = 
• • • , the structure of the reduced system will be that of a presymplectic system 
(M K ,£l K ,H K ) where £l K denotes the restriction of to the final constraints sub- 
space M K and the same for H K . The presymplectic form fl K will have a kernel K K . 
If K K = 0, then the form Q K would be symplectic and we would have obtained a 
consistent and explicit Hamiltonian form for the PMP with a well defined optimal 
feedback law. 

On the other hand if K K ^ then the reduced consistent quasilinear DAE: 

ir K Q K = dH K , (38) 

will be just presymplectic, meaning by that the there would persists an ambiguity 
in the dynamical equations. Such ambiguity will come from K K because if we have 
any consistent determination of ii a = C a (x,p,u) for our system, adding to it any 
vector Z 6 K K will also be a consistent determination for the dynamics. We will call 
such ambiguity in the determination of the dynamics the residual "gauge" symmetry 
of the system because of the resemblance with the case of singular Lagrangians and 
gauge symmetries. We will describe explicitly the dependence of the di ffere ntial 



conditions for the reduced PMP in terms of the constraints (f>^ in Section 3.4 after 
an appropriate extension of the presymplectic algorithm is presented. 

The need for an extension of the presymplectic algorithm comes not only to 
analyze the gauge ambiguity of the reduced PMP but also because the presymplectic 
character of the form £Iq obscures the numerical algorithm due to the singular nature 
of the corresponding matrices. It would be desirable to translate the previous analysis 
into a nonsingular (this is symplectic) setting. This can be done by using a natural 
regularization of (arbitrary) presymplectic systems based in the so called coisotropic 
embedding theorem [10]. The coisotropic embedding theorem roughly states that any 
presymplectic manifold can be embbeded in a symplectic manifold in an essentially 
unique way. We will take advantage of this theorem to transform our presymplectic 



system, either (31) or (32), into a genuine symplectic Hamiltonian system and then 



we will reproduce the recursive constraints analysis discussed in Section 2.2 leading 
to (M K ,Q K ,H K ). 



3.2. Hamiltonian regularization of the presymplectic PMP. Due to the par- 
ticular structure of the presymplectic systems we are dealing with it won't be needed 
to use the coisotropic embedding theorem in its more general abstract setting. In 
fact we will construct explicitly the various spaces needed and the theorem reduces 
to the fact that such choices are natural and unique in an appropriate sense. 

We want to extend the presymplectic manifold (Mo, 0,q) to a symplectic manifold 
(M, Q) as well as the Hamiltonian Hq and the constraints. The total space M is 
obtained adding the dual space of the characteristic distribution Kq of Slo to the 
control degrees of freedom, i.e., M = T*P x C x Kq = T*P x T*C = T*(P x C). 
Notice that the extension M thus constructed is already a symplectic manifold with 
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the canonical symplectic form £1 of T*(P x C). The dual coordinates to the controls 
that we have introduced will be denoted as v a , a = 1, . . . , m. Finally, the symplectic 
extension Q of the presymplectic form Qq has the canonical form: 

Q = dx l A dpi + du a A dv a . 

Moreover an extension of Pontryagine's Hamiltonian to M is given by 

H(x,p,u,v) = H (x,p,u) + C a (x,p,u)v a , (39) 

with C a = C a (x,p,u) arbitrary functions, because the original space Mq is sitting 
inside M as the submanifold defined by the constraints 

(j)^(x,p,u,v) = v a . (40) 

The extended Hamiltonian H defines a Hamiltonian dynamics T given by 

i f n = dH, (41) 

and the vector field T has the expression: 

~ dH d dH d dH d dH d 

+ 



dpi dx l dx l dpi dv a du a du a dv a 
dH d dH d d dH d 

dpi dx % dx % dpi du a du a dv a 

Since we have extended the space, we must impose some constraints to recover the 
original problem. These constraints are given, as it was said before, by the coisotropic 
coordinates v a , eq. (40), i.e., 

M = {(x,p,u,v)eM\(f>W =v a = 0}. (43) 

These new constraints cf)^ will be called zero order constraints (see Table [l] for a 
summary of the two pictures of the PMP: presymplectic and regularized Hamilton- 
ian). 

The vector field F applied to the zero order constraints cj)^ = v a give the primary 
constraints that we had before, eq. Q, 

f(v a ) = -dH/du a = -<j>M. 

Thus, r is tangent to Mq if and only if dH/du a = 0, as we have established previously. 

Now, applying the constraints algorithm to the extended space and denoting M 
by M-i for consistency with the previous notation, we have that eq. (43) reads Mq = 
{(x,p,u,v) G M_i| = 0}. Denoting by iq : Mq — ^M^\ the canonical embedding, 
we have IqQ = £Iq and IqH = Hq. Thus the presymplectic system (Mq,VLq,Hq) 
emerges as the result of applying the presymplectic algorithm to (M, Q, H) with 
respect to the zero order constraints (f>(°\ From there we iterate the presymplectic 
algorithm obtaining the results described in the previous Section. Thus we have: 

Theorem 2. Given the presymplectic system (Mq, D,q, Hq) corresponding to a singu- 
lar optimal control problem, then the recursive presymplectic algorithm of this system 
and the constraints algorithm of the regularized Hamiltonian system (M, f2, H) with 
respect to the constraint submanifold Mq C M are equivalent. 
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Table 1 . The presymplectic and regularized Hamiltonian pictures of 
PMP for singular optimal control problems. 





Presymplectic 


Regularized Hamiltonian 


Phase space 


M = T*P x C 


M_i = M = T*(P x C) 


2-form 


= dx l A dpi 


9, = dx l A dpi + du a A dv a 


Hamiltonian 




H = H + C a v a 


Constraints (k > 1) 
Constraint (k = 0) 


0(fc) _ 


0i O) = V a 



Proof. Consider the submanifold Mq of M. If we impose T^- to be tangent to it, this 
happens only at the points £ 6 M such that Z(H) = 0, where Z G K = ker |m = 
Oq. But kerQo is spanned by the vectors d/du a wich are the Hamiltonian vector 
fields corresponding to the coisotropic coordinates, this is iQ/Q u aQ = dv a . Thus 



will be tangent to M if T n (v a ) = dH/du a (see eq. (42)), but H = H + C a v a , then 



r^(^a) = dHo/du a + VbdC b /du a , thus on Mo, we have r^(w a ) = if and only if 
dHo/du a = 0. Hence the next step of the algorithm will agree with the first step 



of the recursive presymplectic algorithm (see Secction 3.1) and the equivalence is 



proved. □ 

See the Table [l] for a summary of the two pictures of the PMP: presymplectic and 
regularized Hamiltonian. Notice that any extension of Pontryagine's Hamiltonian and 
the constraints would be valid because they would lead to the same final constraint 



submanifold M K C • • • C Mi C Mo C M_i. We have chosen the extension eq. (39) for 
convenience because if provides the simplest setting for the numerical implementation 
of the algorithm. 

3.3. The Hamiltonian nature of the constraints algorithm with partial feed- 
back on the controls. In this section we will show that the constraints algorithm 
with partial feedback on the controls is Hamiltonian, i.e., we prove that at each iter- 
ation the vector field r( fc ) is Hamiltonian, hence the differential conditions describing 
the reduced PMP obtained by using the algorithm are Hamiltonian. 

We have already shown that the presymplectic constraints algorithm given by 



eq. (37), the constraints algorithm eq. (11) and the constraints algorithm with 
partial feedback on the controls, are equivalent (Thm. [I]). However the equivalence 
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was stated at the level of the subspaces determined by the constraints (and the 
constraints themselves) of each one of the algorithms. The dynamics (or differential 
conditions) for each one where determined in a different way. In the constraints 
algorithm, the dynamics was determined by the consistency condition itself that 
precisely selected the points such that a vector that preserves the constraints and 
that belongs to the initial dynamics exists. The presymplectic algorithm determines 



the dynamics by means of the dynamical equation eq. (35) at each iteration (and is 
explicitly Hamiltonian because of this), and the constraints algorithm with partial 
feedback on the controls determines the dynamics at each step by solving part of 
the controls (eq. ([22])). Because the subspaces defined by the presymplectic and 
the constraints algorithm are the same, the presymplectic forms of both coincide and 
because the dynamics defined by the constraints algorithm is a particular instance 
of the original differential condition that satisfies the presymplectic equation, the 
induced dynamics will be Hamiltonian too. However the partial feedback algorithm 
defines the dynamics in a different way but it is Hamiltonian too as it is shown in 
the following proposition: 

Theorem 3. At each iteration of the constraints algorithm with partial feedback on 
the controls it is satisfied: 

where denotes Pontryagine 's Hamiltonian where the partial feedbacks has been 
applied to the controls and is the column vector Vi?^ = 

[dH^/dx T ;8H^/dp T ). 

Proof. We will prove it by induction. At the step zero, it is trivial to show that: 

JT<°) = V#<°> 

where 



r (o) 



X 




. p _ 





Ax + Bu 
- A T p + Nu 



and #(°) :=H . 

As stated in the proposition, is obtained from by applying the partial 

feedback to the controls £t fe_1 , hence by restricting 1) to the subspace Mk C 
Mfc_i. Taking the differential commutes with the restriction map, thus if we apply 
the restriction map to the equation JT^ k ~^ = V-ff^ -1 ' on both sides, we will get 
jr( fc ) = because the restriction of T^ k ^ is exactly (recall that is 

defined just applying the partial feedback to r( fc-1 )). □ 

In what follows we will simply call the "Hamiltonian constraints algorithm" to 
the recursive constraints algorithm with partial feedback on the controls applied to 
the extended space M_i with zero order constraints 4>^ = v a , and no mention will 
be made to the partial feedback on the controls unless it is needed. 

Diagrame [T] summarizes the various algorithms with all the maps and spaces pre- 
sented so far. The left-hand side of the diagrame shows the presymplectic constraints 
algorithm and the right-hand side the partial feedback on the controls. Notice that 
the first row consists on the symplectic space T*P x C that is denoted either by M_i 
or by M^ -1 ) depending if we want to emphasize the starting point of the presym- 
plectic or the partial feedback algorithm respectively. The spaces on the second row 
are again all the same, the total space Mq = T*P x C of the problem, but again, 
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Constraints M_i 
M 



Mi 

( 2 )=0 



'12 



M 2 



p 2n 



x R 2m » = Af( _1 ) Partial feedback 

5(-i) =t ,(°)=o 
Jo 



|2n x R mo 



w=o 



'1 



Mi 



( 2 )=0 



'2 



M 2 



M(°) 

A 

h M(°)=Feed( 1 ' C 

M« 

A 

J2 «( 1 )=Feed( 2 ' C 

M( 2 ) 

A 



M K _i 

<)=o 



M 



K-l 



*)=o 
— M 



A 

J« a( K - 1 )=Feed(' c ' C 



Figure 1. The recursive Hamiltonian and the partial feedback con- 
straints algorithms 



on the left-hand side it appears as the result of applying the zero order constraints 
= to M_i, while in the right-hand side it appears as the result of using the 
partial feedback v a = to the new "control" variables. 

The column on the left shows the sequence of spaces M& = {0( fc ) = 0} and natural 
inclusions : M/% — ^ M^_i obtained by applying the recursive constraint algorithm 
eq. [ll] The central column shows the sequence of spaces M^ = {(f>^ = 0} and 
inclusions ik ■ M& — ^M&_i obtained by applying the partial feedback algorithm eq. 



(23) as well as the isomorphisms ctk between M^ and Mfc, eq. (24) 



Finally, the column on the right shows the sequence of spaces 



o2n 



with the controls left after the previous partial feedbacks and the corresponding 
inclusions j k : AfW — >■ M^ k ~ l \ The bottom row shows the final constraints space 
M K that describes the reduced PMP, or equivalently the space M K which is contained 
in M( K ) which is the space whose points are (x,p,u^), with the set of controls 
that remain free after the reduction process. 

3.4. First and second class constraints and the structure of the reduced 

PMP. Once obtained the reduced presymplectic system (M K ,fi K , H K ) we analyze 



the family of vector fields T K satisfying the dynamical equation eq. (38). 

First we classify the set of constraints {4>^ \ k = 0, 1, 2, . . .} as first or second 
class constraints accordingly with the characteristics of the Hamiltonian vector fields 
defined by them. Again the terminology is reminiscent of the theory of constraints 
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for singular Lagrangian systems (see for instance [19] and references therein). Such 
decomposition of the set of constraints will lead us to a neat characterization of the 
structure of the presymplectic manifold (M K ,Q K ) being symplectic if all constraints 
are second class, while it will possess a nontrivial kernel K K ^ if and only if there 
are non-vanishing first class constraints. 

Consider (M, Vt) a symplectic manifold and a regular submanifold i : N M. 
We will say that a function <f> vanishing on N is a first class constraint if for any 
function tp vanishing on N, = 0, the Poisson bracket of both functions vanish on 
N, i.e., {4>, iP}\n = 0. Otherwise case we will call <p a second class constraint. 

It is well-known that given a family of independent constraints <3? = {4>j \ j = 
l,...p}, we can always decompose it into two subfamilies <3? = {va, Xa \ A = 
1, . . . , r, a = 1, ... ,2s}, r + 2s = p, where va and Xa are first and second class 
constraints respectively and they verify that: 

{v a ,vb\ = C c AB v c , (44) 
{vA,Xa} = C^ a v B , 

{Xa,X(3} = C a/3 

with C a p is non-degenerate (see more details in fl~9]). 

Theorem 4. Given the final reduced presymplectic system (M K ,fl K ,H K ) associated 
to a singular optimal control problem K K = ker Q K is generated by the Hamiltonian 
vector fields T UA corresponding to the first class constraints va- The vector fields T K 
whose integral curves are the optimal extremals of the problem, have the form: 

r 

T k = T Hk + ^2\ a T ua . (45) 

A=l 

where va, A = 1, . . . , r, are the first class constraints of the system. 

Proof. It is a general fact that Hamiltonian vector fields corresponding to first class 
constraints generate the characteristic distribution of the presymplectic form f] K (see 
for instance |19j). 

On the other hand, the most general hamiltonian in M K extending H K is 

H = H K + \ A UA + \ a Xa, 
and the vector field associated to it is 

r = r^ K + \ A v VA + \ a v Xa + v A r xA + x Q i>. 

Thus, restricted to M K this vector field has the form 

t \m k = t h k + A A r^ + A Q r Xa . 

Since this vector field must leave invariant the set of constraints because it must be 
tangent to M K , then: 

T{y B ) = T( Xb ) = 0, 

on M K . But Th k already leaves them invariant by construction, then we have = 
r(x,s) \m k = {^ A va + A a Xa,Xs} \m k = ^C af3 and consequently X a = 0. □ 

Hence the reduced PMP (M K , fl K , H K ) of a singular optimal control problem could 
be either: 
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'1) Nondegenerate: M K is a symplectic submanifold of M = M_i with Q K a 
closed and non-degenerate 2-form and a hamiltonian H K . The vector field 



T K eq. (45), whose integral curves are optimal extremals for the problem is 
uniquely determined and there are no undetermined controls. There must be 
an optimal feedback law determining the controls (even though some of them 
as well as some variables x's and/or p's could have been removed along the 
reduction procedure) . 
(2) Degenerate: M K is a presymplectic submanifold of M = M-\. The 2-form is 
presymplectic and its kernel K K is generated by first class constraints. The 
differential conditions implementing the reduced PMP are undetermined and 
there will be a "gauge" freedom determined by the first class constraints of 



the system with the form given by eq. ( 45 ) with the A being the "effective 
controls" of the theory. The effective controls X A could be either some of the 
original controls u a or new functions of the variables x,p,u. 

In the following sections we will construct a numerical algorithm that will provide 
not only M K but the decomposition of the family of constraints into first and second 
class and the explicit form of T K . 

4. The numerical Hamiltonian constraints algorithm 

4.1. The Hamiltonian constraints algorithm and first and second class con- 
straints. In this section we will complete the numerical implementation of the Hamil- 
tonian constraints algorithm with partial feedback on the controls in the setting of 
singular LQ systems. After the discussion in the previous Section, what is left to 
conclude the implementation of the algorithm is the analysis of the computation of 
first and second class constraints. 

If we denote by {(pi \ i = 1, . . . ,p} the family of constraints describing the subman- 
ifold M K C M (notice that dimM K = 2n + 2m—p), we form the matrix POI = [POIy] 
whose entries are given by: 

POlij = {<t> i ,(f> j }. (46) 



If we restrict the values of its entries to M K , it is clear because of eqs. (44) that 
the kernel of the corresponding matrix will be spanned by the rows corresponding to 
brackets with first class constraints. In fact we obtain: 

Theorem 5. Let {4>i}, i = l,...,p, be an independent set of functions defining a 
a submanifold N of a symplectic manifold M and let these constraints be such that 
they are linear functions in local canonical coordinates of the symplectic manifold M. 
Denote by POIjj = {<fii,<j)j}, i,j = 1, . . . ,p, the p x p constant matrix built as the 
Poisson brackets of the constraints. Let v^ A ' be a basis of the kernel of the matrix 
POI, A = 1, . . . , r, where 2s = p — r is its rank. Let , a = 1, ... ,2s be a family 
of independent vectors completing the basis . Then, the functions 

v 

VA = Y J ^vf\ A = l,...,r, (47) 
i=i 

and 

v 

Xa = Y^4>jwf\ a = l,...,2s, (48) 
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are basis of the first and second class constraints respectively. 

Proof. First, we notice that if 4>j are linear functions in local canonical coordinates 
(q a ,p a ) of the symplectic manifold M, then the Poisson brackets 



poi jk = {<t> j , ( f> k } = j2 



Oj OCpk 0(pj OCp k 



dq a dp a dp a dq° 



are constants. The p x p matrix POI is skew-symmetric, thus its rank must be even, 
say 2s, hence its kernel will have dimension r = p — 2s. 



Now, the constraints ua and Xa defined by eqs. (47) and (48) are a linear com- 
bination of the starting constraints. When computing the Poisson brackets among 
themselves we get: 

j k j \ k / 

j.k \ k / j 



J 

{x«,va} = E^^ a ^^r ) }=E^ a) ( po M A) ) =o - 

k,j k 

From these expressions we conclude that the constraints va are of first class, 
because all the brackets of them with the remaining constraints vanish on N. Finally, 
the brackets of the constraints defined in ( 48 ) with themselves define a nondegenerate 
matrix (of rank 2s): 

C afi = { Xa ,Xp} = E^f^^fc } =T,^ a) {Mk}wjP = POI(w^,wW), 

j,k j,k 

because by construction the matrix POI is non-degenerate when restricted to the 
subspace W = span-fun") | a = 1, . . . , r}, then the matrix C a p is invertible. □ 

We apply the previous results to the regularized Hamiltonian system on M = 
T*(P x C). The total coordinates of the extended symplectic linear space M 2n x 
M 2m problem are (x,p;u,v) where we have added the coisotropic coordinates, v a , as 
column vectors. 

In matrix form the set of constraints {(f> j \ j = 1, . . .p} is written as: 

<&[x,p, u, v}' = 0, 

where the rows of the p x (2n + 2m) matrix $ are the coefficients of the constraints, 
this is the jth row of $ will represent the jth linear constraint (f)j with general form: 

= o-(j, :) * + f3(j, :) * p(:) + p(j, :) * u(:) + u)(j, :) * v(:) 

and, from now on, we just call $ the set of constraints. 



Because of the analysis of the regularized Hamiltonian problem in Section 3.2 and 
Thm. [2J to obtain the reduced PMP starting from the symplectic manifold M we 
must start with the zero order constraints 4>a = forcing these coordinates to 
vanish. In matrix form they are the first m rows of $ and the second set of m rows 
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are given by the set of primary constraints (j>a , o, = 1, ... ,m. Thus the matrix <1> 
begins as follows: 

Omxn O m xn Omxm I-m 

$ = 

. a« 0« P W mxm 
with [cr^ I 0W] = (eq. (pf). 



4.2. Computing the first and second class constraints. Notice that if we have 
a matrix and we add rows or columns the rank can only increase or remain unchanged 
and any family of previously independent rows will keep being independent. Thus, 
at any step of the algorithm, when we add new constraints to $ and we form the 
new matrix { , } , the family of second class constraints can only increase or remain 
unchanged. Thus we can separate at each step the second class constraints by just 
computing the Poisson brackets of the new constraints with the previous first class 
constraints and analyzing the kernel of the corresponding matrix. 

Thus at each step k of the iteration we separate the new second class constraints. 

Denoting the constraints obtained in the step k as: 

&i(k) : set of first class constraints obtained until step k 

&u(k) : set of second class constraints obtained until step k 

POl(k) : brackets' matrix of {<&i(k), $/(£;)} = 

F: new set of constraints. 

We update the matrix F adding the new set of constraints obtained to the first 
class ones: 

F 

We compute the Poisson brackets matrix POI(/c + 1) computing the Poisson brackets 
among the rows of F, this is: 

POI(fc + 1) 

We compute a basis of the kernel of this matrix and we form the matrix: 



F 






{*/(*:), F} " 


{F, $/(£;)} 


{F,F} 



[ 



,(1) 



/ r ) 1 and another matrix whose columns complete the basis v^ A \ 



We denote this matrix asiu = [ wM | • • • | ] . 

The set of first class constraints, eq. (|47|), is given 

$/(& + !) 



v T * F, 



(49) 



and the second class constraints obtained at this step, eq. (48), is given by: 

® n = w T *F. (50) 

Now, we add them to those obtained in previous steps to form the set of second class 
constraints until step k, that is, 



$>ii 



(51) 



Finally we will discuss briefly the reduction of columns suffered by all the con- 
straints at each step of the algorithm due to the partial feedback. Consider a con- 
straint <f> given as 4>(x,p, u, v) = ax + f3p+ pu + uv = 0. The reduction of columns due 
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to the feedback is as follows. After obtaining the feedback of in the first step, 



we have, eq. (13), w ' = 



fi(°)(l : ri ) 



, since u^(l : ri) = Feed^ 1 ** 



x 
P 



wc 



perform the following simultaneous change: 
pu + uov = [pV (1) ](:, 1 : n)Feed (1) 



x 
P 



+ 



+ [pV {1) }(:, ri + 1 : mi)w (1) + [wV (1) ](:, n + 1 : mi))u (1) , 

where we have used the fact that the zeroth constraint {v a } = must vanish, so we 
can get rid of the part [loV^]v^°\ This procedure will be repeated at each step of 
the algorithm and at the end we will remove the remaining coisotropic variables. 



5. Implementation of the algorithm 

In this section we describe the pseudocode for the numerical regularized Hamil- 
tonian constraints algorithm. 



Scheme 1: HCAPF "Hamiltonian constraints algorithm with partial feed- 
back on the controls" (Regularized Hamiltonian recursive constraints algorithm 
splitting first and second class constraints for singular optimal LQ control problems) 



Initialize the variables: S, R; Feed and Nofeed. 

Build the constraints matrix <3? adding the coisotropic variables 

Compute the Poisson brackets matrix POI of 

Split 3> in first and second class constraints 

Regular problems 

Compute the total feedback and final vector field (there are no constraints) 

Singular problems 

while the number of independent constraints increases 
if rank(ii) > 

Compute the partial feedback: Feed, Feedtot and Nofeed 
Compute the vector field: G and Z using the partial feedback 
Include the feedback into the first and second class constraints 
Iterate the matrices for next step 
endif 

Add the new set of constraints to 
Compute the new Poisson bracket matrix POI 
Split the set of constraints in first and second class 
Add the new second class constraints to the previous ones 
endwhile 
Remove the coisotropic variables 



Obtain the final vector field, total feedback, first and second class constraints 
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Pseudocode 1: HCAF "Hamiltonian constraints algorithm with partial 
feedback on the controls" 



input A, B, Q, N, R, tol 

% Initialization of variables: 

S <- [ -N T ,B T ] ; R i R; r ^rank(i?, tol); [l,m] <-size(i?); 

Feedtot = [ ] ; Feed = [ ];% Feedback 



G 



A nX n 

Q -A T 



B 

N 



; [n,n] <— size(A);% Vector Field 



Rfeed <- 0; M <- m; k <- 0; 

% Addition of the coisotropic constraints. 







mx2n Omxrn 



[S I i2] independent _rows ([S \ R],tol); Phi 

S R fflX in 

POI <— poisson_brackets (Phi, n, m, tol); % bracket of the constraints. 
[v,w] ^— numerical_ker (POI, tol); 

Phi/ <— v T Phi; Phi// <— tt; T Phi; % isf and £n<i class constraints. 



% Regular problems 

if rank(i?, tol) == m 

[U T , R, V T ] <-svd(.R); Rfeed <- m; 

Feed <- iT 1 * ?7 * 5; Feedtot = V; 

G <- G + Z * F T ; Nofeed, Phi/, Phi//, Z ^empty ; 



% Singular problems 
else Nofeed «— 7 m ; p «— 0; 

while Rfeed < m & p «— 2r+rank(Phi, io/); 
fc <- fc + 1; 

if r > % There is partial feedback. 

Rfeed <- Rfeed + r; [C/ T , R, V T ] ^svd(R); 
% Partial feedback 
Ri-R(l:r,l : r); 

/: 



Feed <- iT 1 * [7(1 : r, :) * 5; 



Feed 



Feedtot «- 



Feedtot 
V(l : r, :) * Nofeed 



Nofeed <- [ V * Nofeed ] (r + 1 : m, :); 
% Vector field, including feedback 
G^G + Z(:, 1 : r) * Feed; Z <- Z(:,r + 1 : m); 



% Reduction of 1st class constraints due to feedback. 
if rank(Phi/, toZ) > toZ; 

F 3 <- Phi/(:, 2n + 1 : 2n + m) * F T ; 

Phi/(:, 1 : 2n) <- Phi/(:, 1 : 2n) + F30, 1 : r) * Feed; 

F 4 <- [Phi/(:,2n + m+ 1 : 2n + 2m) *V T ](:,r+ 1 : m); 

Phii ^— independent_rows([Phi/(:, 1 : 2n), F 3 (:,r + 1 : m), F 4 ],fo^); 
endif 
.../... 
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Pseudocode 1: HCAPF "Hamiltonian constraints algorithm with partial 
feedback on the controls" 



•••/••• 

% Reduction of 2nd class constraints due to feedback. 
if rank(Phi//, tol) > tol 

F 3 <- Phi//(:, 2n + 1 : 2n + m) * V T ; 

Phi//(:, 1 : 2n) «- Phi//(:, 1 : 2n) + F 3 (:, 1 : r) * Feed; 

F 4 <- [Phi// (:,2n + m+ 1 : 2n + 2m) * V T ](:,r + 1 : m); 

Phi// ^— independent _rows( [Phi// (:, 1 : 2n), F 3 (:,r + 1 : m), F^],tol) 
endif 

% Iteration of the matrices 

R^U(r + l:l,:)*S*Z; 

S <- U{r + 1 : I,:) * S * G; 
elseif % Iteration of the matrices for r = 0. There is no feedback. 

R^- S*Z; 
S*G; 

endif 



r «— rank(i?, tol); [l,m] «— size(-R); p <— rank(Phi, tol) 
% New set of constraints 

Phi/ 



Phi <— independent_rows 



,tol); 



S R Olxm 

POI <— poisson_brackets (Phi, n, m, tol); [v,w] ^— numerical_ker (POI, tol); 
Phi/ <— v T * Phi; % 1st class constraints. 

Phi// «— independent_rows ^ **phi 
endwhile 



, tol ] ; % w/ioZe set 2nd class constraints. 



% Computation of the final feedback for the last iteration 
if r >tol 

% Partial feedback 



Feed <- iT 1 * [7(1 : r, :) * 5; <- 



fxp 

Feed 



Feedtot <- 



Feedtot 
_ V(l : r, :) * Nofeed _ 
Nofeed <- [~V * Nofeed ] (r + 1 :~m, :); 



% Reduction of 1st class constraints due to feedback. 
if rank(Phii, tol) > tol; 

F 3 <- Phi/(:, 2n + 1 : 2n + m) * y T ; 

Phi/(:, 1 : 2n) «- Phi/(:, 1 : 2n) + i^O, 1 : r) * Feed^; 

F 4 <- [Phi/(:,2n + m+ 1 : 2n + 2m) * V T ](:,r + 1 : m )- 

Phi/ ^— independent_rows([Phi/(:, 1 : 2n), F3(:,r + 1 : m), i<4],ioZ); 

endif 

.../... 
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Pseudocode 1: HCAPF "Hamiltonian constraints algorithm with partial 
feedback on the controls" 



•••/••• 

% Reduction of of 2nd class constraints due to feedback. 
if rank(Phi//, tot) > tol 

F 3 <- Phi//(:, 2n + 1 : 2n + m) * V T ; 

Phi/j(:, 1 : 2n) «- Phi//(:, 1 : 2n) + F 3 (:, 1 : r) * Feed; 

F 4 <- [Phi//(:, 2n + m + 1 : 2n + 2m) * F T ](:, r + l:m); 

Phi// ^— independent_rows(Phi//(:, 1 : 2n), i*3(:,r + 1 : m), F4], to/) 

endif 
endif 

Feed <- / xp ; 
endif 

m <— M - Rfeed 

% Remove the coisotropic variables. 

if rank(Phi/, to/) > to/; 

Phi/ <— independent _rows(Phi/(:, 1 : 2n + m),to/); 
endif 

if rank(Phi//, tol) > tol; 

Phi// ^— independent_rows(Phi//(:, 1 : 2n + m), to/); 
endif 

% Fma/ vector field: x — A x x + A p p + B u u;p = Q x x + Q p p + N u u 
A x <r- G{1 : n, 1 : n); A p <- G{1 : n, n + 1 : 2n); 

<- G(n + 1 : 2n, 1 : n); Q p <- G(n + 1 : 2n, n + 1 : 2n); 
if Rfeed< M 

B u <- Z(l : M, :); JV„ <- Z(M + 1 : 2M, :); 
elseif B U ,N U <— empty; 
endif 

Output k, m, Phi/, Phi//, Feed, Feedtot, Nofeed, A x , A p , Q x , Q p , B u , N u ; 



Pseudocode 2: independent_rows used in HCAPF 
procedure independent_rows(<I>, to/) 

if rank($,to/) > to/; 

[l/,S,V]<-svd($); 

r «- rank($, to/); 

$ <- [ U T <5> ] (1 : r,:); 
elseif Q <— empty; 
endif 



output 
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Pseudocode 3: numerical_ker used in HCAPF 



procedure numerical_ker(A,tol) 

[m,n] «— size(A); r <— rank(A, tol); 

[U,S,V] ^— svd(j4); % Singular Value Decomposition of A 

v <— V(:, r + 1 : n); % Matrix whose columns span the kernel of A with tolerance tol 
w V(:, 1 : r); % Matrix whose columns are independent of the columns of v, 
output V, w 



Pseudocode 4: poisson_brackets used in HCAPF 



procedure poisson_brackets(Phi, n, m, tol) 
I ^rows(Phi); POI <- ix /; 
if rank(Phi, tol) > tol 
for i = 1 : n 

POI <- POI + Phi(:, i) * Phi(:, i + n) T - Phi(:, i + n) * Phi(:, i) T ; 

% Contribution of the state and costate variables, (x,p). 
endfor 
for j = 1 : m 

POI <- POI + Phi(:,j + 2n) * Phi(:, j + 2n + m) T - Phi(:,j + 2n + m) * Phi(:, j + 2n) T 
% Contribution of the control and coisotropic variables, (u,v). 
endfor 

POI-POF 
2 

elseif 

POI <- [ ] 

endif 

output POI 



Above we show the pseudocodes for this algorithm as well as the subroutines 
used in it: independent _rows, numerical_ker, and poisson_brackets. The func- 
tion independent_rows remove the dependent rows of a matrix The function 
numerical_ker computes a matrix v whose columns span the kernel of A and a matrix 
w whose columns are independent of the columns of v. Finally, poisson_brackets 
generates the matrix POI = (POI^), where each element POL^ is the Poisson bracket 
of the i-th row with the j'-th row of the matrix <F The dimensions n and m of the 
state and control variables, x and u, are needed to compute the Poisson bracket. 



6. Examples and numerical experiments 

We discuss here some numerical experiments that exhibit the stability and con- 
sistency of the numerical algorithm discussed above. The microprocessor used for 
the numerical computations was Intel(R), Core(TM) i7-2640M, CPU 2.80 GHz, 2.80 
GHz, 4.00 GB RAM, 64 bits, and the program was MATLAB 7.12.0. 
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We describe two sets of experiments with small and large recursive index respec- 
tively. We test the algorithm against a class of non-trivial problems general enough 
for the purposes of exhibiting its numerical stability but that can be solved exactly. 

In the small index problems (y = 2), we show that the algorithm is stable with 
respect to the tolerance used to compute the numerical rank, tol, and with respect 
to perturbations of the data, 8. We will also discuss the dependence with the size of 
the matrices, n. 

In the large index case, we analyze a problem of index n — 1, where the algorithm 
behaves properly with respect to the number of steps, both regarding the tolerance, 
tol, and the perturbation of the data, 5. 

6.1. Small index problems. Small matrices. Consider a positive semidefinite 
symmetric n x n matrix R of rank r, thus there will exist an orthogonal matrix U 
such that 

R = U T R'U, (52) 

where all elements of R' vanish except R'n, ■ ■ ■ ,R' rr > 0. State and control spaces 
are both R n , and the total space (x,p, u) is M 3ra . The matrix A is the identity and B 
is an orthogonal matrix, B T B = I n . Finally, the objective functional is constructed 
by using a generic diagonal matrix Q = D, a matrix N of the form N = BV, where 
V is a symmetric matrix constructed as V = \B T ■ D\ ■ B, where Di is the truncated 
diagonal matrix of Q with only the / first diagonal entries non-zero, and the matrix R 
described above. The primary constraints, taking into account that B T N — N T B = 
(without feedback) are: 

0« = -N T x + B T p -Ru = 0, 
an we will obtain the partial feedback of r controls. The reduced constraint will be: 

4fV (x, p, u) = U l (-N T A + B T Q)x - U l B T A T p = 0, 
and computing the derivative we get: 

= U\-N T A 2 + B T QA- B T A T Q)x + U l B T {A T fp+ (53) 
+ U 1 (B T QB - N T AB - B T A T N)u = 0. 

The matrix B T QB - N T AB - B T A T N will be invertible for generic A, Q, B and 
V, but for our problem the matrix R^ = U 1 (B T QB — 2V) has rank m — /, so we 
get partial feedback of m — I controls and the algorithm stops here since the rest is 
proportional to the previous constraints. 

The numerical experiment of this problem will consist in applying the algorithm 
to a collection of matrices built up as a random perturbation of size 5 of the matrices 
A, B, Q, N: A = A + 5 A, \\SA\\ < 5, B = B + 5B, \\SB\\ <5,Q = Q + 5Q, \\SQ\\ < 5 
and N = N + 6N, \\SN\\ < 5. It is computed for n = 100, 200, with r = n - 20 and 
I = 5. 

We analyze the number of steps before the algorithm stabilizes and compute the 
angle a, between the final constraint submanifold of the perturbed problem and the 
exact one, this is the error introduced in the problem by the perturbations 6A, 5B, 5Q 
and SN. Notice that the original matrix R does not affect higher order constraints, 
hence its numerical influence restricts just to launching the algorithm until they are 
of order of the tolerance, tol = 10~ 6 . 
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Table 2. First experiment (small index, small matrices), tol = 10 



n 


5 


# exact steps 


# steps 


m 


mi 


rp 


rpi 


a/ error 


100 


le-013 


3 


3 


15 


15 


10 


10 


0.0000000000561788 


100 


le-012 


3 


3 


15 


15 


10 


10 


0.0000000005405419 


100 


le-011 


3 


3 


15 


15 


10 


10 


0.0000000054350352 


100 


le-010 


3 


3 


15 


15 


10 


10 


0.0000000548428769 


100 


le-009 


3 


3 


15 


15 


10 


10 


0.0000004943661747 


100 


le-008 


3 


3 


15 


15 


10 


10 


0.0000049043295974 


100 


le-007 


3 


3 


15 


15 


10 


12 


0.0000373718753680 


100 


le-006 


3 


3 


15 





10 


10 


not computable 


100 


le-005 


3 


2 


15 





10 


2 


not computable 


200 


le-013 


3 


3 


15 


15 


10 


10 


0.0000000000108757 


200 


le-012 


3 


3 


15 


15 


10 


10 


0.0000000001127705 


200 


le-011 


3 


3 


15 


15 


10 


10 


0.0000000010570457 


200 


le-010 


3 


3 


15 


15 


10 


10 


0.0000000113724130 


200 


le-009 


3 


3 


15 


15 


10 


10 


0.0000001093589043 


200 


le-008 


3 


3 


15 


15 


10 


10 


0.0000011856677989 


200 


le-007 


3 


7 


15 


15 


10 


22 


0.0000068162846885 


200 


le-006 


3 


3 


15 





10 


12 


not computable 


200 


le-005 


3 


2 


15 





10 





not computable 



Table[2]shows the exact and computed steps of the algorithm, the leftover controls, 
m = n—(r+l) = n— (n— 20+5) = 15 and the rank rp = 10 of the Poisson matrix, that 
gives us the number of second class constraints. We compare it with the perturbed 
problem (number of steps, mi and rpi). 

Finally, we compute the error (a), measured as the angle between both set of 
constraints. This is only computable when both set of constraints have the same 
number of columns, so it fails when the leftover controls are not the same. The 
results in Table [2] show that the algorithm works well up to perturbations of order of 
5 = 10~ 6 = tol, even though the nature of first/second class constraints fails at 5 = 
10~ 7 and so does the number of steps for n = 200. The least squares approximation 
of lo g 10 a versus log 10 5 gives two lines of slopes 0.9765 (for n = 100)and 0.9803 (for 
n = 200), which is consistent with a = 0(5). (See Figure [2]). 

In this experiment it was also computed the improvement obtained by perform- 
ing partial feedback on the controls at each step versus the plain constraints algo- 
rithm. When matrices are large enough the improvement in CPU time is in the range 
[58.1%, 118.4%] for n = 100 and [76.1%, 200.4%] for n = 200. 

In Table § we have selected a fixed value for the perturbation 5 (= 10 12 ). We 
can observe that the algorithm is almost insensitive to the size of the matrices, n. 
Again, the improvement in computation time due to partial feedback is in the range 
of [84.9%, 171.6%]. 



6.2. Small index problems. Large matrices. Let us consider now the linear- 
quadratic problem given by Q = A = I n G R nxn , B T = (1, . . . , 1) e M nxl , N T = 
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Figure 2. Error for the first experiment (small index, small matri- 
ces), n = 100, 200, tol = 1(T 6 



Table 3. First experiment (small index, small matrices). 5 = 10 
tol = W' 6 



n 


# exact steps 


# steps 


m 


mi 


rp 


rpi 


a/ error 


100 


3 


3 


15 


15 


10 


10 


0.0000000000464235 


125 


3 


3 


15 


15 


10 


10 


0.0000000004122544 


150 


3 


3 


15 


15 


10 


10 


0.0000000000829511 


175 


3 


3 


15 


15 


10 


10 


0.0000000011055558 


200 


3 


3 


15 


15 


10 


10 


0.0000000001268241 



(0, . . . , 0) 6 R n , R = 0; so the constraints matrix will be (as shown in [6]) 





" •• 


• 


•• 








1 " 


$ = 


•• 


• 


1 •• 


1 








1 •• 


• 1 


-1 •• 


• -1 










•• 


• 


1 •• 


1 


n 






(54) 



where n is the dimension of the matrices A and Q. Thus, in the third row we 
obtain optimal feedback and the final constraint submanifold is given by the following 
equations: x\ + ■ ■ ■ + x n = p\ + ■ ■ ■ + p n = u = 0. 

We apply the numerical algorithm for the previous matrices for n = 3000, toler- 
ance equal to 10~ 6 and we compare the solution obtained with the perturbed matri- 
ces: A = A + SA, \\SA\\ < 5, N = N + 6N, \\SN\\ < 5, B = B + 5B, \\SB\\ < 5, and 
Q = Q + 5Q, \\SQ\\ < 5, where 5 = 10~ 13 — *- 10~ 5 . 

Again, as the final constraint submanifold of the original problem and the per- 
turbed one must be the same, we measure the angle between them (the error), we 
show it in Table [4j The slope of the least squares approximation of log 10 (a) ver- 
sus log 10 (<5) is 0.9988 (see Figure [3]). Again, consistently with the previous results, 
the data shows that a = 0(8), and fails when the perturbation is of order of the 
tolerance. 



A HAMILTONIAN ALGORITHM FOR SINGULAR LQ SYSTEMS 31 



Table 4. Second experiment, n = 3000. tol = 10 



5 


# exact steps 


# steps 


m 


mi 


rp 


rpi 


a/ error 


le-013 


3 


3 








2 


2 


0.0000000000022437 


le-012 


3 


3 








2 


2 


0.0000000000223028 


le-011 


3 


3 








2 


2 


0.0000000002226745 


le-010 


3 


3 








2 


2 


0.0000000022331076 


le-009 


3 


3 








2 


2 


0.0000000223615340 


le-008 


3 


3 








2 


2 


0.0000002228797000 


le-007 


3 


3 








2 


2 


0.0000022084665700 


le-006 


3 


3 








2 


2 


0.0000218159062779 


le-005 


3 











2 





not computable 



-3 



logio(a) 



Figure 3. Error for the second experiment (small index, large ma- 
trices), n = 3000. tol = 10~ 6 



6.3. Large index problems. Consider the following problem: 



A G M nxn , Q = A + A 
As shown in [6j, = p (2) = • • • 



B g 



snxl 



N = B, R = 0. 



(55) 



0, since all these matrices are always zero, 



there will not be optimal feedback and the constraints matrix will be: 



-B T 
B T A T 
-B T {A T ) 2 









1 " 


B T 








-B T A T 








B T {A T ) 2 








l) k B T {A T ) k 









-(-l) k B T (A T ) k (- 

The algorithm will stop when at a given step we obtain a linear combination of 
the previous rows. We apply the algorithm to a problem where the pair (A, B) is 
such that A G IR" - *™ is a nilpotent matrix of index n, i.e., A n ~ l ^ 0, A n = 0, and 
B $ ker(A'), I = 1, . .. ,n- 1, i.e., B T = (1, . . . , 1) G M lx ™. Thus the index of the 
algorithm is n — 1 (n steps). For n = 20, 5 = 10 -13 — >■ 10 -5 , and tolerance equal to 
10~ 6 , we perturb the matrices as: A = A + 5A, \\5A\\ < 8, N = N + SN, \\SN\\ < 5, 
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B = B + 5B, \\SB\\ < 5 and Q = A + A T , \\SQ\\ < 5. The perturbation of Q is 
done to maintain the structure of the problem. The results are shown in Tableland 
Figure |6T3} The slope of the least squares fitting of the resulting data is 1.0115, that 
is, a = 0(5) (see Figure Q. 



Table 5. Third experiment (large index), tol = 10 



n 


5 


# exact steps 


# steps 


m 


mi 


rp 


rpi 


a/ error 


20 


le-013 


20 


20 


1 


1 








0.0000000000005502 


20 


le-012 


20 


20 


1 


1 








0.0000000000053287 


20 


le-011 


20 


20 


1 


1 








0.0000000000891883 


20 


le-010 


20 


20 


1 


1 








0.0000000003152048 


20 


le-009 


20 


20 


1 


1 








0.0000000039064143 


20 


le-008 


20 


20 


1 


1 








0.0000000375324515 


20 


le-007 


20 


20 


1 


1 





2 


0.0000011726145723 


20 


le-006 


20 


3 


1 








2 


not computable 


20 


le-005 


20 





1 











not computable 




Figure 4. Error for the third experiment (large index), n = 20, tol = 10 
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